/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.archwood.frc2607.logomotion;

import com.sun.squawk.microedition.io.FileConnection;
import edu.wpi.first.wpilibj.Timer;
import java.io.IOException;
import java.io.PrintStream;
import java.util.Vector;
import javax.microedition.io.Connector;

/**
 *
 * @author Ron
 */
public class LogomotionTelemetry {

    private Vector loggedObjects;
    private PrintStream telemFile;
    private boolean headerWritten;
    private int entryCount;
    private Thread m_task;
    private boolean runThread, loggingEnabled;
    private long sleepTime;
    
    public LogomotionTelemetry() {
        loggedObjects = new Vector();
        String fileName = "file:///output-" + String.valueOf(System.currentTimeMillis()) +
                          ".csv";
        try {
            FileConnection fc = (FileConnection)Connector.open(fileName,
                                                Connector.WRITE);
            fc.create();
            telemFile = new PrintStream(fc.openOutputStream());
        }
        catch (IOException ex) {
            telemFile = null;
        }
        headerWritten = false;
        entryCount = 0;
        runThread = true;
        loggingEnabled = false;
        sleepTime = 20;
        initThread();
    }

    private void initThread() {
        m_task = new TelemetryThread(this);
        m_task.start();
    }
    
    public void addLoggedObject(LogomotionConstants obj) {
        loggedObjects.addElement(obj);
    }

    public void setLoggingInterval(long ms) {
        sleepTime = ms;
    }

    public void stopLogging() {
        loggingEnabled = false;
    }

    public void startLogging() {
        loggingEnabled = true;
    }
    
    public void doLogEntry() {
        if (telemFile == null || loggedObjects.isEmpty()) return;
        if (entryCount > 9000) return;
        String entry;
        int i;
        entryCount += 1;
        if (!headerWritten) {
            headerWritten = true;
            entry = "Time,FPGATime";
            i = 0;
            while(i < loggedObjects.size()) {
                entry += ",";
                entry += ((LogomotionConstants)loggedObjects.elementAt(i++)).getTelemetryHeader();
            }
            telemFile.println(entry);
        }
        entry = Long.toString(System.currentTimeMillis()) + "," +
                Double.toString(Timer.getFPGATimestamp());
        i = 0;
        while (i < loggedObjects.size()) {
            entry += ",";
            entry += ((LogomotionConstants)loggedObjects.elementAt(i++)).getTelemetryData();
        }
        telemFile.println(entry);
        telemFile.flush();
    }

    
    private class TelemetryThread extends Thread {

        LogomotionTelemetry telem;

        TelemetryThread(LogomotionTelemetry t) {
            telem = t;
        }

        public void run() {
            while (runThread) {
                if (loggingEnabled) telem.doLogEntry();
                try { Thread.sleep(sleepTime); } catch (Exception e) {}
            }
        }
    }
    
}
